/**
 * \file
 * \date 17.09.2013
 * \author alex
 */
#ifndef bullet_rviz_test_HEIGHTFIELD_VISUALIZATION_H_
#define bullet_rviz_test_HEIGHTFIELD_VISUALIZATION_H_

#include <stdint.h>
#include <boost/shared_ptr.hpp>
#include <geometry_msgs/Point.h>
#include <std_msgs/ColorRGBA.h>
#include <bullet_rviz_test/visual_object.h>

namespace bullet_rviz_test
{

class Heightfield;
typedef boost::shared_ptr<Heightfield> HeightfieldPtr;

class HeightfieldVisualization: public VisualObject
{
public:
  HeightfieldVisualization(const HeightfieldPtr & heightfield, uint32_t color);
  virtual ~HeightfieldVisualization();

  virtual void visualize();

private:
  HeightfieldVisualization(const HeightfieldVisualization &);
  HeightfieldVisualization & operator=(const HeightfieldVisualization &);

  static void fillPoint(geometry_msgs::Point & point,
      geometry_msgs::Point::_x_type x, geometry_msgs::Point::_y_type y,
      geometry_msgs::Point::_z_type z);
  std_msgs::ColorRGBA colorFromHeight(float height);
  bool isOk(geometry_msgs::Point::_z_type z);

  HeightfieldPtr heightfield_;
  uint32_t color_;
};

}

#endif /* bullet_rviz_test_HEIGHTFIELD_VISUALIZATION_H_ */
